#include "../common/irobot_abstract.h"
#include "../common/oi.h"

int main(void){
	ir_initialize(0);
	ir_sleep(1000);
	ir_send_byte(CmdStart);
	ir_send_byte(CmdFull);

/// light the leds--idiot test
	ir_sleep(1000);
	ir_send_byte(CmdLeds);
	ir_send_byte(0);
	ir_send_byte(0);
	ir_send_byte(255);


	// No bumper pressed = toggle LEDs
	
	int toggle = 1;
	unsigned char b = 0;
	while (2){
		ir_sleep(100);

		if(toggle){
			toggle = 0;
		}
		else{
			toggle = 1;
		}

		// Left bumper = left LED lights
		ir_send_byte(CmdSensors);
		ir_send_byte(7);
		unsigned char c = ir_get_byte();
		switch (c){
			case BumpLeft:
				//printf("Get your hands off my left bumper!\n");
				printf("mushroom mushroom\n");
				ir_send_byte(CmdLeds);
				ir_send_byte(LEDPlay);
				ir_send_byte(0);
				ir_send_byte(255);
				break;
			case BumpRight:
				//printf("Get your hands off my right bumper!\n");
				printf("Mushroom Mushroom\n");
		 	      	ir_send_byte(CmdLeds);
				ir_send_byte(LEDAdvance);
				ir_send_byte(0);
				ir_send_byte(255);
				break;
			case BumpBoth:
				//printf("No really, my bumpers are my business!\n");
				printf("Ah!!! Its a SNAKE!!\n");
		 	      	ir_send_byte(CmdLeds);
				ir_send_byte(LEDsBoth);
				ir_send_byte(0);
				ir_send_byte(255);
				break;
			default:
				ir_send_byte(CmdLeds);
				ir_send_byte(0);
				ir_send_byte(0);
				ir_send_byte(255);
				
				if(toggle){
					printf("badger\n");
					ir_send_byte(CmdLeds);
					ir_send_byte(LEDPlay);
					ir_send_byte(0);
					ir_send_byte(255);
					ir_sleep(150);
				}
				else{
					printf("Badger\n");
					ir_send_byte(CmdLeds);
					ir_send_byte(LEDAdvance);
					ir_send_byte(0);
					ir_send_byte(255);
					ir_sleep(150);
				}
				break;
				

		}

	    
		// Play button = move clockwise along 25 cm square
		// Advance button = counterclockwise along 25 cm square		
		ir_send_byte(CmdSensors);
		ir_send_byte(18);		
		b = ir_get_byte();
		if(b == ButtonAdvance || b == ButtonPlay){
			ir_send_byte(CmdLeds);
			ir_send_byte(0);
			ir_send_byte(128);
			ir_send_byte(128);
			ir_sleep(250);

			int i;
			for(i = 1; i <= 4; i++){
				printf("Full speed ahead\n");
				//move forward 25cm
				ir_send_byte(CmdDriveWheels);
				//drive at 250mm/s
				//0000 0000 1111 1010 (250)
				ir_send_byte(0);
				ir_send_byte(0x7d);
				ir_send_byte(0);
				ir_send_byte(0x7d);
				ir_sleep(2000);

				//turn clockwise
				ir_send_byte(CmdDriveWheels);
				if(b == ButtonAdvance){
					printf("Turn left\n");
					ir_send_byte(0);
					ir_send_byte(0xdd);
					ir_send_byte(0xff);
					ir_send_byte(0x23);
				}
				if(b == ButtonPlay){
					printf("Turn right\n");
					ir_send_byte(0xff);
					ir_send_byte(0x23);
					ir_send_byte(0);
					ir_send_byte(0xdd);
				}
				ir_sleep(955);

			}
		
			//stop spinning?
			printf("And...stop.\n");
			ir_send_byte(CmdDriveWheels);
			ir_send_byte(0);
			ir_send_byte(0);
			ir_send_byte(0);
			ir_send_byte(0);

		}
	}



	ir_finish();

	return 0;
}
